#include "Spring_Dynamic_lyvm.h"
#include "animationCenter.h"

#include <cmath>
#include <sstream>

#define M 100.0 //質量
#define MAX_V 500.0 //系統的最大速度
bool bFirst = true;
DIVA_VM* firstVM;
int _posBound;

extern HINSTANCE _hInst ;  // this instance is defined in DIVA_MAIN
extern HWND _hWnd ;        // this instance is defined in DIVA_Application

extern int ds_k;
extern int ds_rf;
extern int ds_sl;
extern int ds_as;

Spring_Dynamic_lyvm::Spring_Dynamic_lyvm(){
	_flag = false;
	_name = "Spring Dynamic Layout";
	animationCenter::getInstance()->addAnimation(this);
}

void Spring_Dynamic_lyvm::setupDialog(){

	addDescript("This is the Spring Layout Dialog:");
	addParameter("Spring Factor","int","15");
	addParameter("Friction","int","300");
	addParameter("Spring Length","int","500");
	addParameter("Animation Speed","int","3");
	addParameter("Position Bound","int","900");
	//addDescript("VM無法移動超過座標值Position Bound的範圍");
}

void Spring_Dynamic_lyvm::setupParameters(){
	string sf_s = getParameter("Spring Factor");
	istringstream(sf_s)>>_spring_factor;
	string f_s = getParameter("Friction");
	istringstream(f_s)>>_friction;
	string sl_s = getParameter("Spring Length");
	istringstream(sl_s)>>_spring_length;
	string as_s = getParameter("Animation Speed");
	istringstream(as_s)>>_animation_speed;
	string pb_s = getParameter("Position Bound");
	istringstream(pb_s)>>_posBound;
}

Ogre::Vector3 getDiffVector(Ogre::Vector3 a, Ogre::Vector3 b){
	Ogre::Vector3 ret = Ogre::Vector3(a.x-b.x, a.y-b.y, a.z-b.z);
	return ret;
}

float computeLen(float x, float y, float z){
	float squar = 2.0; 
	float squar_root = 0.5;
	float temp = pow(x, squar)+pow(y, squar)+pow(z, squar);
	float ret = pow(temp, squar_root);

	return ret;
}

float computeDistance(Ogre::Vector3 a, Ogre::Vector3 b){
	Ogre::Vector3 dV = getDiffVector(a, b);
	return computeLen(dV.x, dV.y, dV.z);
}

float retard(float v, float fct){
	if(abs(v)<fct)
		return 0.0;
	if(v>0)
		return v - fct;
	if(v<0)
		return v + fct;
	// TODO throw exception
	assert( !"invalid range" );
	return 0.0f;
}

void Spring_Dynamic_lyvm::behavior(float avgFrameTime){

	if(this->_flag == false) return;
	map<Link*,PositionMoveState*>::iterator itr;	
	pair<Link*,PositionMoveState*> p;
	float ani_rate = avgFrameTime*_animation_speed;

	for(itr=anis.begin(); itr!=anis.end(); itr++){
		p = (*itr);
		DIVA_VM* aniVM = p.first->toVM;
		Ogre::Vector3 direct = p.second->direct;
		//防止VM移動速度過快
		if(p.second->vel>MAX_V) p.second->vel=MAX_V;

		float v = p.second->vel;
		float moveX = aniVM->getPosition().x + direct.x * v * ani_rate;
		float moveY = aniVM->getPosition().y + direct.y * v * ani_rate;
		float moveZ = aniVM->getPosition().z + direct.z * v * ani_rate;

		if(abs(moveX)>2*_posBound||abs(moveY)>2*_posBound||abs(moveZ)>2*_posBound)
			continue;

		aniVM->setPosition(moveX,moveY,moveZ);
		_record[aniVM] = Ogre::Vector3(moveX,moveY,moveZ);
		layout(_area->getAllVMs(),aniVM);	

		float fct = p.second->resist;
		p.second->vel = retard(v, fct*ani_rate);//計算延遲

	}

	//////////////ad1339   防止移動過遠的機制 ////////////////////
	vector<DIVA_VM*>::iterator itr2;
	vector<DIVA_VM*> allVMs = _area->getAllVMs();
	DIVA_VM* vm;
	int sunX = 0;
	int sunY = 0;
	int sunZ = 0;
	int avgX, avgY, avgZ;
	int area_size = allVMs.size();

	for(itr2 = allVMs.begin(); itr2!=allVMs.end(); itr2++){
		vm = (*itr2);
		Ogre::Vector3 vMpos = vm->getWorldPosition();
		sunX += vMpos.x;
		sunY += vMpos.y;
		sunZ += vMpos.z;
	}

	if(area_size == 0) return;

	avgX = sunX / area_size;
	avgY = sunY / area_size;
	avgZ = sunZ / area_size;

	if(abs(avgX)>_posBound||abs(avgY)>_posBound||abs(avgZ)>_posBound){
		for(itr2 = allVMs.begin(); itr2!=allVMs.end(); itr2++){
			vm = (*itr2);
			Ogre::Vector3 vMpos = vm->getPosition();
			vm->setPosition(vMpos.x-avgX,vMpos.y-avgY,vMpos.z-avgZ);
			_record[vm] = vMpos;
		}
	}
	/////////////////////////////////////////////////////////////

}

PositionMoveState* merge(PositionMoveState* pms1, PositionMoveState* pms2){
	PositionMoveState* ret;

	float tmpV_x = ((pms1->direct).x)*(pms1->vel) + ((pms2->direct).x)*(pms2->vel);
	float tmpV_y = ((pms1->direct).y)*(pms1->vel) + ((pms2->direct).y)*(pms2->vel);
	float tmpV_z = ((pms1->direct).z)*(pms1->vel) + ((pms2->direct).z)*(pms2->vel);

	float l = computeLen(tmpV_x,tmpV_y,tmpV_z);//計算長度

	if(l == 0.0)
		l = 0.001;

	Ogre::Vector3 tmpV = Ogre::Vector3(tmpV_x/l,tmpV_y/l,tmpV_z/l);
	float res = (pms1->resist + pms2->resist) /2 ; //摩擦力是個常數,取誰都可以

	ret = new PositionMoveState(tmpV, l, res);

	return ret;
}

void Spring_Dynamic_lyvm::createSprForceFromVMtoVMs(DIVA_VM* rootVM, vector<DIVA_VM*> vms){
	vector<DIVA_VM*>::iterator itr;

	DIVA_VM* vm;
	//這個迴圈在做的事情是針對一個VM所能影響的VM們做出應有的拉力行為
	for(itr=vms.begin(); itr!=vms.end(); itr++){
		vm = (*itr);

		float newL = computeDistance(rootVM->getPosition(), vm->getPosition());//計算距離 
		//長度差距 決定力的大小
		float dL = newL - _spring_length;
		//牛頓第二運動定律,加速度依照 F=kx=ma 的物理公式算出
		float a = _spring_factor * dL / M; 

		Ogre::Vector3 dV = getDiffVector(rootVM->getPosition(), vm->getPosition());
		//方向的單位向量
		Ogre::Vector3 direct = Ogre::Vector3(dV.x/newL, dV.y/newL, dV.z/newL);  

		PositionMoveState* pms = new PositionMoveState(direct, a, _friction);

		map<Link*,PositionMoveState*>::iterator itr_m;
		pair<Link*,PositionMoveState*> p_m;
		bool bFound = false;

		for(itr_m=anis.begin();itr_m!=anis.end();itr_m++){
			p_m = (*itr_m);
			//如果這個速度來源是已存在的,則與前速度向量結合
			if(p_m.first->fromVM==rootVM && p_m.first->toVM==vm){
				//牛頓第一運動定律-慣性定律,與前慣性向量做結合
				pms = merge(pms, p_m.second);
				p_m.second->setup(pms);

				bFound = true;
				break;
			}
		}
		//如果這個速度來源是新的,則新增一個速度向量
		if(!bFound){
			Link* link = new Link;
			link->fromVM = rootVM;
			link->toVM = vm;
			anis.insert(make_pair(link,pms));
		}
	}
}

void Spring_Dynamic_lyvm::layout(vector<DIVA_VM*> movedVMs,DIVA_VM* movedVM){
	//movedVMs沒作用
	//移動的VM對其餘VM的拉力
	vector<DIVA_VM*> infVMs = _area->getVMsLinkedByVM(movedVM);	
	createSprForceFromVMtoVMs(movedVM, infVMs);

	//牛頓第三運動定理,關於牽動這個VM該受到反作用力(繩子的反饋力)
	vector<DIVA_VM*> preVMs = _area->getVMsLinkToVM(movedVM);
	createSprForceFromVMtoVMs(movedVM, preVMs);
	
	return;
}